# include <cstdlib>
# include <iostream>
# include <iomanip>
# include <cmath>
# include <fstream>
# include <cstring>

using namespace std;

# include "tri.hh"
# include "utils.hh"

int i4_modp ( int i, int j )
{
  int value;

  if ( j == 0 )
  {
    cout << "\n";
    cout << "I4_MODP - Fatal error!\n";
    cout << "  I4_MODP ( I, J ) called with J = " << j << "\n";
    exit ( 1 );
  }

  value = i % j;

  if ( value < 0 )
  {
    value = value + abs ( j );
  }

  return value;
}

int i4_wrap ( int ival, int ilo, int ihi )
{
  int jhi;
  int jlo;
  int value;
  int wide;

  jlo = i4_min ( ilo, ihi );
  jhi = i4_max ( ilo, ihi );

  wide = jhi + 1 - jlo;

  if ( wide == 1 )
  {
    value = jlo;
  }
  else
  {
    value = jlo + i4_modp ( ival - jlo, wide );
  }

  return value;
}


/*
//  Purpose:
//
//    BASIS_11_T3: one basis at one point for a T3 element.
//
//  Discussion:
//
//    The routine is given the coordinates of the nodes of a triangle.
//
//           3
//          / \
//         /   \
//        /     \
//       1-------2
//
//    It evaluates the linear basis function Q(I)(X,Y) associated with
//    node I, which has the property that it is a linear function
//    which is 1 at node I and zero at the other two nodes.
//
//  Parameters:
//
//    Input, double T[2*3], the coordinates of the nodes.
//
//    Input, int I, the index of the desired basis function.
//    I should be between 1 and 3.
//
//    Input, double P[2], the coordinates of the point where 
//    the basis function is to be evaluated.
//
//    Output, double *QI, *DQIDX, *DQIDY, the value of the I-th basis function
//    and its X and Y derivatives.
*/
double* Tri3::evaluate_basis ( int n_points, const double p[])
{
  double *phi = new double[3*n_points];

  int ip1, ip2;

  for (int j = 0; j < n_points; j++ )
  {
    ip1 = 2; ip2 = 3;
    phi[j*3+0] = ( ( vertices[0+(ip2-1)*3] - vertices[0+(ip1-1)*3] ) 
                 * ( p[1+j*3]           - vertices[1+(ip1-1)*3] ) 
                 - ( vertices[1+(ip2-1)*3] - vertices[1+(ip1-1)*3] ) 
                 * ( p[0+j*3]           - vertices[0+(ip1-1)*3] ) ) / volume;

    ip1 = 3; ip2 = 1;
    phi[j*3+1] = ( ( vertices[0+(ip2-1)*3] - vertices[0+(ip1-1)*3] ) 
                 * ( p[1+j*3]           - vertices[1+(ip1-1)*3] ) 
                 - ( vertices[1+(ip2-1)*3] - vertices[1+(ip1-1)*3] ) 
                 * ( p[0+j*3]           - vertices[0+(ip1-1)*3] ) ) / volume;

    ip1 = 1; ip2 = 2;
    phi[j*3+2] = ( ( vertices[0+(ip2-1)*3] - vertices[0+(ip1-1)*3] ) 
                 * ( p[1+j*3]           - vertices[1+(ip1-1)*3] ) 
                 - ( vertices[1+(ip2-1)*3] - vertices[1+(ip1-1)*3] ) 
                 * ( p[0+j*3]           - vertices[0+(ip1-1)*3] ) ) / volume;

  }

  return phi;
}

double* Tri3::evaluate_gradient ( int n_points, const double points[] )
{
  double *dphi = new double[3*2*n_points];

  int ip1;
  int ip2;

  // the derivatives are arranged as
  // [dphi_dx_1, dphi_dx_2, .. dphi_dx_np, dphi_dy_1, dphi_dy_2, ... dphi_dy_np]

  for (int j = 0; j < n_points; j++ )
  {
    ip1 = 2; ip2 = 3;
    dphi[j*3+0] = - ( vertices[1+(ip2-1)*3] - vertices[1+(ip1-1)*3] ) / volume;
    dphi[(n_points+j)*3+0] =   ( vertices[0+(ip2-1)*3] - vertices[0+(ip1-1)*3] ) / volume;

    ip1 = 3; ip2 = 1;
    dphi[j*3+1] = - ( vertices[1+(ip2-1)*3] - vertices[1+(ip1-1)*3] ) / volume;
    dphi[(n_points+j)*3+1] =   ( vertices[0+(ip2-1)*3] - vertices[0+(ip1-1)*3] ) / volume;

    ip1 = 1; ip2 = 2;
    dphi[j*3+2] = - ( vertices[1+(ip2-1)*3] - vertices[1+(ip1-1)*3] ) / volume;
    dphi[(n_points+j)*3+2] =   ( vertices[0+(ip2-1)*3] - vertices[0+(ip1-1)*3] ) / volume;
  }

  return dphi;

}

int Tri3::test ( )
{
# define NODE_NUM 3

  int i;
  int j;
  double sum_x;
  double sum_y;

  cout << "\nBASIS_11_T3_TEST:\n";
  cout << "  Verify basis functions for element T3.\n\n";
  cout << "  Number of nodes = " << NODE_NUM << "\n";

  cout << "\n  Physical Nodes:\n\n";
  for ( j = 0; j < NODE_NUM; j++ )
  {
    cout << "  "
         << setw(10) << vertices[0+j*3] << "  "
         << setw(10) << vertices[1+j*3] << "  "
         << setw(10) << vertices[2+j*3] << "\n";
  }
 
  cout << "\n  The basis function values at basis nodes\n";
  cout << "  should form the identity matrix.\n\n";

  double* phi = this->evaluate_basis(NODE_NUM, vertices);

  for ( i = 0; i < NODE_NUM; i++ )
  {
    cout << "  " << setw(10) << phi[i*NODE_NUM] <<  "  " << setw(10) << phi[i*NODE_NUM+1] <<  "  " << setw(10) << phi[i*NODE_NUM+2] << "\n";
  }
  delete phi;

  cout << "\n  The X and Y derivatives should sum to 0.\n\n";
  cout << "  dPhidX sum, dPhidY sum:\n\n";

  double* dphi = this->evaluate_gradient(NODE_NUM, vertices);
  for ( i = 0; i < NODE_NUM; i++ )
  {
    sum_x = 0.0;
    sum_y = 0.0;
    for ( j = 0; j < NODE_NUM; j++ )
    {
      sum_x = sum_x + dphi[j];
      sum_y = sum_y + dphi[NODE_NUM+j];
    }
    cout << "  "
         << setw(10) << sum_x << "  "
         << setw(10) << sum_y << "\n";
  }

  delete dphi;

  return 0;
# undef NODE_NUM
}


/*
//  Purpose:
//
//    BASIS_MN_T4: one basis at one point for a T4 element.
//
//  Discussion:
//
//    The T4 element is the cubic bubble triangle.
//
//    The routine is given the coordinates of the vertices of a triangle.
//    It works directly with these coordinates, and does not refer to a 
//    reference element.
//
//    The sides of the triangle DO NOT have to lie along a coordinate
//    axis.
//
//    The routine evaluates the basis functions associated with each vertex,
//    and their derivatives with respect to X and Y.
//
//  Physical Element T4: 
//       
//            3
//           / \
//          /   \
//         /  4  \
//        /       \
//       1---------2
//
//  Licensing:
//
//    This code is distributed under the GNU LGPL license. 
//
//  Modified:
//
//    07 August 2009
//
//  Author:
//
//    John Burkardt
//
//  Parameters:
//
//    Input, double T[2*4], the coordinates of the vertices
//    of the triangle, and the coordinates of the centroid.  
//    It is common to list the first three points in counter clockwise
//    order.
//
//    Input, int I, the index of the basis function.
//
//    Input, double P[2], the point where the basis function
//    is to be evaluated.
//
//    Output, double *PHI, the value of the basis function
//    at the evaluation point.
//
//    Output, double *DPHIDX, *DPHIDY, the value of the 
//    derivatives at the evaluation point.
//
//  Local parameters:
//
//    Local, double AREA, is (twice) the area of the triangle.
*/
double* Tri4::evaluate_basis ( int n_points, const double p[])
{
  double dpsidx[4];
  double dpsidy[4];
  int n = 4;
  double psi[4];
  
  double *phi = new double[4*n_points];

  for (int k = 0; k < n_points; k++ )
  {

    psi[0] =     (   ( vertices[0+2*3] - vertices[0+1*3] ) * ( p[k*3+1] - vertices[1+1*3] )     
                   - ( vertices[1+2*3] - vertices[1+1*3] ) * ( p[k*3+0] - vertices[0+1*3] ) );
    dpsidx[0] =    - ( vertices[1+2*3] - vertices[1+1*3] );
    dpsidy[0] =      ( vertices[0+2*3] - vertices[0+1*3] );

    psi[1] =     (   ( vertices[0+0*3] - vertices[0+2*3] ) * ( p[k*3+1] - vertices[1+2*3] )     
                   - ( vertices[1+0*3] - vertices[1+2*3] ) * ( p[k*3+0] - vertices[0+2*3] ) );
    dpsidx[1] =    - ( vertices[1+0*3] - vertices[1+2*3] );
    dpsidy[1] =      ( vertices[0+0*3] - vertices[0+2*3] );

    psi[2] =     (   ( vertices[0+1*3] - vertices[0+0*3] ) * ( p[k*3+1] - vertices[1+0*3] )     
                   - ( vertices[1+1*3] - vertices[1+0*3] ) * ( p[k*3+0] - vertices[0+0*3] ) );
    dpsidx[2] =    - ( vertices[1+1*3] - vertices[1+0*3] );
    dpsidy[2] =      ( vertices[0+1*3] - vertices[0+0*3] );

  //
  //  Normalize the first three functions.
  //
    for (int j = 0; j < 3; j++ )
    {
      psi[j]    = psi[j]    / volume;
      dpsidx[j] = dpsidx[j] / volume;
      dpsidy[j] = dpsidy[j] / volume;
    }

  //
  //  Compute the cubic bubble function.
  //
    psi[3] = 27.0 * psi[0] * psi[1] * psi[2];

    dpsidx[3] = 27.0 * (
                    dpsidx[0] *    psi[1] *    psi[2]
                    +  psi[0] * dpsidx[1] *    psi[2]
                    +  psi[0] *    psi[1] * dpsidx[2] );

    dpsidy[3] = 27.0 * (
                    dpsidy[0] *    psi[1] *    psi[2]
                    +  psi[0] * dpsidy[1] *    psi[2]
                    +  psi[0] *    psi[1] * dpsidy[2] );
  //
  //  Subtract 1/3 of the cubic bubble function from each of the three linears.
  //
    for (int j = 0; j < 3; j++ )
    {
      phi[k*n+j] =    psi[j] -    psi[3] / 3.0;
//      dpsidx[j] dphi[k*n+j] = dpsidx[j] - dpsidx[3] / 3.0;
//      dpsidy[j] dphi[(n_points+k)*n+j] = dpsidy[j] - dpsidy[3] / 3.0;
    }

    phi[k*n+3] =    psi[3] ;
//    dphi[k*n+3] = dpsidx[3] ;
//    dphi[(n_points+k)*n+3] = dpsidy[3] ;
  }

  return phi;
}


double* Tri4::evaluate_gradient ( int n_points, const double p[])
{
  double dpsidx[4];
  double dpsidy[4];
  double psi[4];

  int n = 4;
  
  double *phi = new double[4*n_points];
  double *dphi = new double[4*2*n_points];

  for (int k = 0; k < n_points; k++ )
  {
    psi[0] =     (   ( vertices[0+2*3] - vertices[0+1*3] ) * ( p[k*3+1] - vertices[1+1*3] )     
                  - ( vertices[1+2*3] - vertices[1+1*3] ) * ( p[k*3+0] - vertices[0+1*3] ) );
    dpsidx[0] =    - ( vertices[1+2*3] - vertices[1+1*3] );
    dpsidy[0] =      ( vertices[0+2*3] - vertices[0+1*3] );

    psi[1] =     (   ( vertices[0+0*3] - vertices[0+2*3] ) * ( p[k*3+1] - vertices[1+2*3] )     
                  - ( vertices[1+0*3] - vertices[1+2*3] ) * ( p[k*3+0] - vertices[0+2*3] ) );
    dpsidx[1] =    - ( vertices[1+0*3] - vertices[1+2*3] );
    dpsidy[1] =      ( vertices[0+0*3] - vertices[0+2*3] );

    psi[2] =     (   ( vertices[0+1*3] - vertices[0+0*3] ) * ( p[k*3+1] - vertices[1+0*3] )     
                  - ( vertices[1+1*3] - vertices[1+0*3] ) * ( p[k*3+0] - vertices[0+0*3] ) );
    dpsidx[2] =    - ( vertices[1+1*3] - vertices[1+0*3] );
    dpsidy[2] =      ( vertices[0+1*3] - vertices[0+0*3] );

  //
  //  Normalize the first three functions.
  //
    for (int j = 0; j < 3; j++ )
    {
      psi[j]    = psi[j]    / volume;
      dpsidx[j] = dpsidx[j] / volume;
      dpsidy[j] = dpsidy[j] / volume;
    }

  //
  //  Compute the cubic bubble function.
  //
    psi[3] = 27.0 * psi[0] * psi[1] * psi[2];

    dpsidx[3] = 27.0 * (
                    dpsidx[0] *    psi[1] *    psi[2]
                    +  psi[0] * dpsidx[1] *    psi[2]
                    +  psi[0] *    psi[1] * dpsidx[2] );

    dpsidy[3] = 27.0 * (
                    dpsidy[0] *    psi[1] *    psi[2]
                    +  psi[0] * dpsidy[1] *    psi[2]
                    +  psi[0] *    psi[1] * dpsidy[2] );
  //
  //  Subtract 1/3 of the cubic bubble function from each of the three linears.
  //
    for (int j = 0; j < 3; j++ )
    {
      phi[k*n+j] =    psi[j] -    psi[3] / 3.0;
      dphi[k*n+j] = dpsidx[j] - dpsidx[3] / 3.0;
      dphi[(n_points+k)*n+j] = dpsidy[j] - dpsidy[3] / 3.0;
    }

    phi[k*n+3] =    psi[3] ;
    dphi[k*n+3] = dpsidx[3] ;
    dphi[(n_points+k)*n+3] = dpsidy[3] ;

  }

  return dphi;
}

int Tri4::test ()
{
# define NODE_NUM 4

  int i, j;
  double sum_x, sum_y;
  
  cout << "\nBASIS_11_T4_TEST:\n";
  cout << "  Verify basis functions for element T4.\n\n";
  cout << "  Number of nodes = " << NODE_NUM << "\n";

  cout << "\n  Physical Nodes:\n\n";
  for ( j = 0; j < NODE_NUM; j++ )
  {
    cout << "  "
         << setw(10) << vertices[0+j*2] << "  "
         << setw(10) << vertices[1+j*2] << "  "
         << setw(10) << vertices[2+j*2] << "\n";
  }
 
  cout << "\n  The basis function values at basis nodes\n";
  cout << "  should form the identity matrix.\n\n";
  
  double* phi = this->evaluate_basis(NODE_NUM, vertices);

  for ( i = 0; i < NODE_NUM; i++ )
  {
    cout << "  " << setw(10) << phi[i*NODE_NUM] <<  "  " << setw(10) << phi[i*NODE_NUM+1] <<  "  " 
                 << setw(10) << phi[i*NODE_NUM+2] <<  "  " << setw(10) << phi[i*NODE_NUM+3] << "\n";
  }

  delete phi;
  
  cout << "\n  The X and Y derivatives should sum to 0.\n\n";
  cout << "  dPhidX sum, dPhidY sum:\n\n";

  double* dphi = this->evaluate_gradient(NODE_NUM, vertices);

  for ( i = 0; i < NODE_NUM; i++ )
  {
    sum_x = 0.0;
    sum_y = 0.0;
    for ( j = 0; j < NODE_NUM; j++ )
    {
      sum_x = sum_x + dphi[j];
      sum_y = sum_y + dphi[NODE_NUM+j];
    }
    cout << "  "
         << setw(10) << sum_x << "  "
         << setw(10) << sum_y << "\n";
  }

  return 0;
# undef NODE_NUM
}



/*
//
//  Purpose:
//
//    BASIS_11_T6: one basis at one point for the T6 element.
//
//  Discussion:
//
//    The routine is given the coordinates of the nodes of a triangle. 
//       
//           3
//          / \
//         6   5
//        /     \
//       1---4---2
//
//    It evaluates the quadratic basis function B(I)(X,Y) associated with
//    node I, which has the property that it is a quadratic function
//    which is 1 at node I and zero at the other five nodes.
//
//    This routine assumes that the sides of the triangle are straight,
//    so that the midside nodes fall on the line between two vertices.
//
//    This routine relies on the fact that each basis function can be
//    written as the product of two linear factors, which are easily
//    computed and normalized.
//
//  Parameters:
//
//    Input, double T[2*6], the coordinates of the nodes.
//
//    Input, int I, the index of the desired basis function.
//    I should be between 1 and 6.
//
//    Input, double P[2], the coordinates of a point at which the basis
//    function is to be evaluated.
//
//    Output, double *BI, *DBIDX, *DBIDY, the values of the basis function
//    and its X and Y derivatives.
//
*/
double* Tri6::evaluate_basis ( int n_points, const double p[])
{
  double gf, gn, hf, hn;
  int j1, j2, k1, k2;

  double *phi = new double[6*n_points];

  for (int j = 0; j < n_points; j++ )
    for (int ii = 1; ii <= 6; ii++ )
    {
      int i = ii;
      //
      //  Determine the pairs of nodes.
      //
      if ( i <= 3 )
      {
        j1 = i4_wrap ( i + 1, 1, 3 );
        j2 = i4_wrap ( i + 2, 1, 3 );
        k1 = i + 3;
        k2 = i4_wrap ( i + 5, 4, 6 );
      }
      else
      {
        j1 = i - 3;
        j2 = i4_wrap ( i - 3 + 2, 1, 3 );
        k1 = i4_wrap ( i - 3 + 1, 1, 3 );
        k2 = i4_wrap ( i - 3 + 2, 1, 3 );
      }

    //
    //  For C++ indexing, it is helpful to knock the indices down by one.
    //
      i  = i  - 1;
      j1 = j1 - 1;
      j2 = j2 - 1;
      k1 = k1 - 1;
      k2 = k2 - 1;
    //
    //  Evaluate the two linear factors GF and HF, 
    //  and their normalizers GN and HN.
    //
      gf = ( p[0+j*3]      - vertices[0+j1*3] ) * ( vertices[1+j2*3] - vertices[1+j1*3] ) 
        - ( vertices[0+j2*3] - vertices[0+j1*3] ) * ( p[1+j*3]      - vertices[1+j1*3] );

      gn = ( vertices[0+i*3]  - vertices[0+j1*3] ) * ( vertices[1+j2*3] - vertices[1+j1*3] ) 
        - ( vertices[0+j2*3] - vertices[0+j1*3] ) * ( vertices[1+i*3]  - vertices[1+j1*3] );

      hf = ( p[0+j*3]      - vertices[0+k1*3] ) * ( vertices[1+k2*3] - vertices[1+k1*3] ) 
        - ( vertices[0+k2*3] - vertices[0+k1*3] ) * ( p[1+j*3]      - vertices[1+k1*3] );

      hn = ( vertices[0+i*3]  - vertices[0+k1*3] ) * ( vertices[1+k2*3] - vertices[1+k1*3] ) 
        - ( vertices[0+k2*3] - vertices[0+k1*3] ) * ( vertices[1+i*3]  - vertices[1+k1*3] );

    //
    //  Construct the basis function and its derivatives.
    //
      phi[j*6+i] =  ( gf / gn ) * ( hf / hn );
     
    }

  return phi;
}

double* Tri6::evaluate_gradient ( int n_points, const double points[] )
{
  double gf, gn, hf, hn;
  int j1, j2, k1, k2;

  double *dphi = new double[6*2*n_points];

  for (int j = 0; j < n_points; j++ )
    for (int ii = 1; ii <= 6; ii++ )
    {
      int i = ii;
      //
      //  Determine the pairs of nodes.
      //
      if ( i <= 3 )
      {
        j1 = i4_wrap ( i + 1, 1, 3 );
        j2 = i4_wrap ( i + 2, 1, 3 );
        k1 = i + 3;
        k2 = i4_wrap ( i + 5, 4, 6 );
      }
      else
      {
        j1 = i - 3;
        j2 = i4_wrap ( i - 3 + 2, 1, 3 );
        k1 = i4_wrap ( i - 3 + 1, 1, 3 );
        k2 = i4_wrap ( i - 3 + 2, 1, 3 );
      }

    //
    //  For C++ indexing, it is helpful to knock the indices down by one.
    //
      i  = i  - 1;
      j1 = j1 - 1;
      j2 = j2 - 1;
      k1 = k1 - 1;
      k2 = k2 - 1;
    //
    //  Evaluate the two linear factors GF and HF, 
    //  and their normalizers GN and HN.
    //
      gf = ( points[0+j*3]      - vertices[0+j1*3] ) * ( vertices[1+j2*3] - vertices[1+j1*3] ) 
        - ( vertices[0+j2*3] - vertices[0+j1*3] ) * ( points[1+j*3]      - vertices[1+j1*3] );

      gn = ( vertices[0+i*3]  - vertices[0+j1*3] ) * ( vertices[1+j2*3] - vertices[1+j1*3] ) 
        - ( vertices[0+j2*3] - vertices[0+j1*3] ) * ( vertices[1+i*3]  - vertices[1+j1*3] );

      hf = ( points[0+j*3]      - vertices[0+k1*3] ) * ( vertices[1+k2*3] - vertices[1+k1*3] ) 
        - ( vertices[0+k2*3] - vertices[0+k1*3] ) * ( points[1+j*3]      - vertices[1+k1*3] );

      hn = ( vertices[0+i*3]  - vertices[0+k1*3] ) * ( vertices[1+k2*3] - vertices[1+k1*3] ) 
        - ( vertices[0+k2*3] - vertices[0+k1*3] ) * ( vertices[1+i*3]  - vertices[1+k1*3] );

    //
    //  Construct the basis function and its derivatives.
    //
      dphi[j*6+i]  =   ( ( vertices[1+j2*3] - vertices[1+j1*3] ) / gn ) 
                * (   hf                      / hn )
                + (   gf                      / gn ) 
                * ( ( vertices[1+k2*3] - vertices[1+k1*3] ) / hn );

      dphi[(n_points+j)*6+i]  = - ( ( vertices[0+j2*3] - vertices[0+j1*3] ) / gn ) 
                * (   hf                      / hn )
                - (   gf                      / gn ) 
                * ( ( vertices[0+k2*3] - vertices[0+k1*3] ) / hn );
     
    }

  return dphi;
}

int Tri6::test ( )
{
# define NODE_NUM 6

  int i;
  int j;
  double sum_x;
  double sum_y;

  cout << "\nBASIS_11_T6_TEST:\n";
  cout << "  Verify basis functions for element T6.\n\n";
  cout << "  Number of nodes = " << NODE_NUM << "\n";

  cout << "\n  Physical Nodes:\n\n";
  for ( j = 0; j < NODE_NUM; j++ )
  {
    cout << "  "
         << setw(10) << vertices[0+j*3] << "  "
         << setw(10) << vertices[1+j*3] << "  "
         << setw(10) << vertices[2+j*3] << "\n";
  }
 
  cout << "\n  The basis function values at basis nodes\n";
  cout << "  should form the identity matrix.\n\n";

  double* phi = this->evaluate_basis(NODE_NUM, vertices);

  for ( i = 0; i < NODE_NUM; i++ )
  {
    cout << "  " << setw(10) << phi[i*NODE_NUM] <<  "  " << setw(10) << phi[i*NODE_NUM+1] <<  "  " << setw(10) << phi[i*NODE_NUM+2] 
         <<  "  " << setw(10) << phi[i*NODE_NUM+3] <<  "  " << setw(10) << phi[i*NODE_NUM+4] <<  "  " << setw(10) << phi[i*NODE_NUM+5] << "\n";
  }
  delete phi;

  cout << "\n  The X and Y derivatives should sum to 0.\n\n";
  cout << "  dPhidX sum, dPhidY sum:\n\n";

  double* dphi = this->evaluate_gradient(NODE_NUM, vertices);
  for ( i = 0; i < NODE_NUM; i++ )
  {
    sum_x = 0.0;
    sum_y = 0.0;
    for ( j = 0; j < NODE_NUM; j++ )
    {
      sum_x = sum_x + dphi[j];
      sum_y = sum_y + dphi[NODE_NUM+j];
    }
    cout << "  "
         << setw(10) << sum_x << "  "
         << setw(10) << sum_y << "\n";
  }

  delete dphi;

  return 0;
# undef NODE_NUM
}


